#ifndef MATCHPROJECTIONPOINT_H
#define MATCHPROJECTIONPOINT_H

#include <ros/ros.h>
#include <iostream>
#include <vector>
#include <utility>
#include <cmath>
#include <limits>
#include <algorithm>
#include <nav_msgs/Odometry.h>
#include "paths_struct.h"
#include "cached_path_profile.h"

class Match_Projection_Point
{
public:
    Match_Projection_Point() = default;
    ~Match_Projection_Point() = default;
    PointAndIndex match_and_projection_point(const nav_msgs::Odometry& odom_receive_, 
                                                                    const std::vector<std::pair<double, double>>& points);
    std::pair<double, double> CalculateProjectionPoint(const std::pair<double, double>& current_odom, 
                                                            const std::pair<double, double>& closest_point, 
                                                            const std::vector<std::pair<double, double>>& xy_points);
    std::pair<double, double> VectorFromPointToPoint(const std::pair<double, double>& pointA, 
                                                                                                                const std::pair<double, double>& pointB);
    double DistanceBetweenPoints(const std::pair<double, double>& pointA, 
                                        const std::pair<double, double>& pointB);
    bool ComputePathProfileWithCache(const std::vector<std::pair<double, double>> &xy_points, 
                                            CachedPathProfile &cache);

private:
    int last_closest_index = -1;
    std::pair<double, double> projection_point;

};

#endif // MATCHPROJECTIONPOINT_H